;----------------------------------------------------------------
;内容
;	サブルーチンの所要時間を測るためのルーチンです。
;	スーパーバイザモードで呼び出して下さい。
;	Timer-CとTimer-Dを使用するので,Timer-Dが使用中の場合は停止しておいて下さい。
;
;呼び出し手順
;	move.l	#param,-(sp)	;初期化ルーチンのパラメータ(ループ回数など)
;	pea.l	tini		;後始末ルーチン(不要ならば0)
;	pea.l	init		;初期化ルーチン(不要ならば0)
;	pea.l	main		;計測するルーチン
;	jsr	optime		;d0.l=計測するルーチンの所要時間(μs単位で12799まで)
;	lea.l	(16,sp),sp
;
;リンク時の注意事項
;	lk -e16のように16バイトアラインメントでリンクして下さい。
;
;各ルーチンの設定
;	各ルーチンはスーパーバイザモードで呼ばれます。
;	sspは復帰アドレスをプッシュする前に16バイト単位にアラインメントされています。
;	初期化ルーチンから後始末ルーチンまでd0-d7/a0-a5を自由に使って構いません。
;	a6レジスタはサブルーチンのアドレスとして使用していますが,破壊しても構いません。
;	初期化ルーチンでレジスタに設定した値はa6を除いてそのまま計測するルーチンに渡されます。
;	例えば,初期化ルーチンでカウンタを初期化し,計測ルーチンでループを開始できます。
;
;各ルーチンの呼び出し順序
;	初期化ルーチン→計測するルーチン→後始末ルーチンの順序で,2回呼び出します。
;	1回目は各ルーチンを命令キャッシュに乗せるための準備です。
;	2回目の,計測するルーチンの所要時間をμs単位で計測します。
;	なお,所要時間には呼び出しと復帰の時間が含まれています。
;	一連の呼び出しの間,割り込みは止められています。
;
;計測結果
;	計測結果はd0.lにμs単位で格納されます。
;	最大計測時間は12799μsです。これを超えると正確に計測できません。
;	50MHzのとき1clock=0.02μsですから,適当な回数だけループさせるなどして下さい。
;----------------------------------------------------------------

	.cpu	68000

;----------------------------------------------------------------
;MFPのアドレス
GPIPDR	equ	$00E88001
AER	equ	$00E88003
DDR	equ	$00E88005
IERA	equ	$00E88007
IERB	equ	$00E88009
IPRA	equ	$00E8800B
IPRB	equ	$00E8800D
ISRA	equ	$00E8800F
ISRB	equ	$00E88011
IMRA	equ	$00E88013
IMRB	equ	$00E88015
VECTR	equ	$00E88017
TACR	equ	$00E88019
TBCR	equ	$00E8801B
TCDCR	equ	$00E8801D
TADR	equ	$00E8801F
TBDR	equ	$00E88021
TCDR	equ	$00E88023
TDDR	equ	$00E88025
SCR	equ	$00E88027
UCR	equ	$00E88029
RSR	equ	$00E8802B
TSR	equ	$00E8802D
UDR	equ	$00E8802F

;----------------------------------------------------------------
;ベクタアドレス
TIMER_D_VECTOR			equ	$0110

;----------------------------------------------------------------
;間接サブルーチンコール(000用)
jsrr	.macro	dst
	.local	next
	move.l	dst,-(sp)	;dspがspを参照している場合があるので最初にプッシュする
	move.l	(sp),-(sp)
	move.l	#next,(4,sp)
	rts
next:
	.endm

;----------------------------------------------------------------
;パラメータの内容
	.offset	0
p_pc		.ds.l	1	;復帰アドレス
p_main:		.ds.l	1	;計測するルーチン
p_init:		.ds.l	1	;初期化ルーチン
p_tini:		.ds.l	1	;後始末ルーチン
p_param:	.ds.l	1	;初期化ルーチンのパラメータ(ループ回数など)
	.text

;----------------------------------------------------------------
;スタックの内容
	.offset	0
v_main:		.ds.l	1	;計測するルーチン
v_init:		.ds.l	1	;初期化ルーチン
v_tini:		.ds.l	1	;後始末ルーチン
v_param:	.ds.l	1	;初期化ルーチンのパラメータ(ループ回数など)
v_ssp:		.ds.l	1	;元のスタックポインタ(この位置+16以上)
v_test:		.ds.b	1	;0=本番,-1=リハーサル
v_tcdcr:	.ds.b	1	;TCDCRのワーク
v_imrb:		.ds.b	1	;IMRBのワーク
v_ierb:		.ds.b	1	;IERBのワーク
	.align	16
v_size:
	.text

;----------------------------------------------------------------
;計測ルーチン
;	スーパーバイザモードで呼び出すこと
;<(4,sp).l:計測するルーチン
;<(8,sp).l:初期化ルーチン(不要ならば0)
;<(12,sp).l:後始末ルーチン(不要ならば0)
;>d0.l:所要時間(単位はμs,0〜12799),-1=Timer-Dが使用中
;----------------------------------------------------------------
	.even
_optime::
	movem.l	d1-d7/a0-a6,-(sp)	;ここではa6も必要
	lea.l	(4*14,sp),a6		;pcのアドレス

;Timer-Dの使用状態の確認
	btst.b	#4,IERB
	beq	@f			;Timer-Dは使用されていない
	movea.l	$1C20.w,a0		;Humanの先頭
	move.l	(8,a0),d0		;Humanの末尾
	cmp.l	TIMER_D_VECTOR,d0
	bcc	@f			;Timer-DはHumanが使用中
	moveq.l	#-1,d0			;Timer-DはHuman以外が使用中
	bra	99f
@@:

;割り込み禁止
	move.w	sr,-(sp)
	ori.w	#$0700,sr

;ワークの確保とアラインメント
	move.l	sp,d0
	and.l	#$0000000F,d0
	neg.l	d0
	move.l	sp,(-v_size+v_ssp,sp,d0.l)	;割り込みを止めてあるので,
	lea.l	(-v_size,sp,d0.l),sp	;この間に割り込みなどでスタックが壊れることはない

;各ルーチンのアドレスをセット
	move.l	(p_main,a6),(v_main,sp)	;計測するルーチン

	lea.l	(dummy_routine,pc),a0
	move.l	(p_init,a6),d0
	beq	@f
	movea.l	d0,a0
@@:	move.l	a0,(v_init,sp)		;初期化ルーチン

	lea.l	(dummy_routine,pc),a0
	move.l	(p_tini,a6),d0
	beq	@f
	movea.l	d0,a0
@@:	move.l	a0,(v_tini,sp)		;後始末ルーチン

	move.l	(p_param,a6),(v_param,sp)	;初期化ルーチンのパラメータ

;タイマ設定
	move.b	IERB,(v_ierb,sp)
	move.b	IMRB,(v_imrb,sp)
	move.b	TCDCR,(v_tcdcr,sp)

	andi.b	#%11001111,IERB		;Timer-C/D割り込み停止
	andi.b	#%11001111,IMRB		;Timer-C/D割り込み禁止

	sf.b	TCDCR			;Timer-C/Dカウント停止
@@:	tst.b	TCDCR			;完全に停止するまで待つ
	bne	@b

;000〜010と020〜060で処理を分ける
@@:	moveq.l	#1,d0
	.cpu	68020
	and.b	(@b-1,pc,d0.l*2),d0
	.cpu	68000
	bne	measure_020

;000〜010用の計測ルーチン
measure_000:
	st.b	(v_test,sp)		;リハーサル

retry_000:
	movea.l	(v_init,sp),a6
	move.l	(v_param,sp),-(sp)
	jsr	(a6)			;初期化ルーチンを呼ぶ
	addq.l	#4,sp

	sf.b	TCDR			;Timer-Cカウンタクリア
@@:	tst.b	TCDR			;カウンタが更新されるまで待つ
	bne	@b
	sf.b	TDDR			;Timer-Dカウンタクリア
@@:	tst.b	TDDR			;カウンタが更新されるまで待つ
	bne	@b

	movea.l	(v_main,sp),a6		;計測するルーチン

;カウント開始
;		  TCCR TDCR
	move.b	#%0111_0001,TCDCR	;Timer-C/Dカウント開始
					;	Timer-Cは1/200プリスケール(50μs)
					;	Timer-Dは1/4プリスケール(1μs)

;計測するルーチンを呼び出す
	jsr	(a6)

;カウント停止
	sf.b	TCDCR			;Timer-C/Dカウント停止
@@:	tst.b	TCDCR			;完全に停止するまで待つ
	bne	@b

	movea.l	(v_tini,sp),a6
	jsr	(a6)			;後始末ルーチンを呼ぶ

	not.b	(v_test,sp)
	beq	retry_000

	bra	measure_done

;020〜060用の計測ルーチン
	.cpu	68020
measure_020:
	st.b	(v_test,sp)		;リハーサル

retry_020:
	movea.l	(v_init,sp),a6
	move.l	(v_param,sp),-(sp)
	jsr	(a6)			;初期化ルーチンを呼ぶ
	addq.l	#4,sp

	sf.b	TCDR			;Timer-Cカウンタクリア
@@:	tst.b	TCDR			;カウンタが更新されるまで待つ
	bne	@b
	sf.b	TDDR			;Timer-Dカウンタクリア
@@:	tst.b	TDDR			;カウンタが更新されるまで待つ
	bne	@b

	movea.l	(v_main,sp),a6		;計測するルーチン

	bra	@f
	.align	16
	.ds.w	16-3
@@:

;カウント開始
;		  TCCR TDCR
	move.b	#%0111_0001,TCDCR	;Timer-C/Dカウント開始
					;	Timer-Cは1/200プリスケール(50μs)
					;	Timer-Dは1/4プリスケール(1μs)

;計測するルーチンを呼び出す
	jsr	(a6)

;カウント停止
	sf.b	TCDCR			;Timer-C/Dカウント停止
@@:	tst.b	TCDCR			;完全に停止するまで待つ
	bne	@b

	movea.l	(v_tini,sp),a6
	jsr	(a6)			;後始末ルーチンを呼ぶ

	not.b	(v_test,sp)
	beq	retry_020

	.cpu	68000

;計測終わり
measure_done:

;タイマ取得
	moveq.l	#0,d0
	moveq.l	#0,d1
	sub.b	TCDR,d0			;Timer-Cカウント数
	sub.b	TDDR,d1			;Timer-Dカウント数(オーバーフローあり)

;タイマ後始末
	move.b	#200,TCDR		;Timer-Cカウンタ復元
	move.b	#20,TDDR		;Timer-Dカウンタ復元
	move.b	(v_imrb,sp),IMRB
	move.b	(v_ierb,sp),IERB
	move.b	(v_tcdcr,sp),TCDCR

;ワークの開放
	movea.l	(v_ssp,sp),sp		;元のスタックポインタ

;割り込み許可
	move.w	(sp)+,sr

;カウンタを合成する
	mulu.w	#50,d0
	cmp.b	d1,d0
	bls	@f
	add.w	#$0100,d0
@@:	move.b	d1,d0
	subq.w	#1,d0

99:	tst.l	d0
	movem.l	(sp)+,d1-d7/a0-a6
	rts

dummy_routine:
	rts